

#include <gaussian_elim.h>

#include <stdlib.h>

/**
 * Use guassian elimination to generate a row-echelon form matrix from
 * the given matrix.
 */
vm_matrix *vm_gaussian_elim(vm_matrix *mtx){

  /* For all the columns: find a row that has a non zero element in that */
  /* column. This will be the pivot. Swap this row to the new top. From  */
  /* here we eliminate all the number in the column we are working on.   */

  int j = 0;
  int row_position = 0;

  for  ( ; j < mtx->columns - 1; j++){

    int i = row_position;
    int row_offset = -1;
    for ( ; i < mtx->rows; i++){
      if (mtx->data[i][j] != 0){
	row_offset = i;
	break;
      }
    }
    
    if (row_offset == -1) continue;

    /* Now that we have found this row, we will move it to the new top.  */
    vm_swap_rows(mtx, i, row_position); // This should work.

    /* Iterate over the rows below and eliminate the current column.     */
    vm_matrix_row elim_row;
    for ( i = row_position+1; i < mtx->rows; i++){
      // Find the elimination factor for this row.
      double c = mtx->data[i][j] / mtx->data[row_position][j];

      vm_get_matrix_row(mtx, row_position, &elim_row);
      _vm_scale_mtx_row(&elim_row, -1 * c);
      _vm_add_rc_to_mtx(mtx, &elim_row, i);
      free(elim_row.data);
    }

    row_position++;

  }

  return mtx;

}

int _vm_add_rc_to_mtx(vm_matrix *mtx, vm_matrix_rc *row, int r_offset){

  if (row->elems != mtx->columns) return 0;
  if (r_offset < 0 || r_offset >= mtx->rows) return 0;

  int i = 0;
  for ( ; i < mtx->columns; i++){
    mtx->data[r_offset][i] += row->data[i];
  }

  return 1;

}

void _vm_scale_mtx_row(vm_matrix_rc *row, double factor){

  int i = 0;
  for ( ; i < row->elems; i++){
    row->data[i] *= factor;
  }

}

vm_matrix *vm_row_echelon_form(vm_matrix *mtx){

  vm_matrix *ref = vm_gaussian_elim(mtx);

  if (!ref) return NULL;

  /* Iterate over the rows and just divide by the pivot. */
  int i = 0;
  for ( ; i < ref->rows; i++){

    int j = 0;
    double pivot = 0;
    int found = 0;
    
    /* Find the pivot elem. */
    for ( ; j < ref->columns; j++){
      if (mtx->data[i][j]){
	pivot = mtx->data[i][j];
	found = 1;
	break;
      }
    }
    
    /* If there was a pivot, then divide the row by that pivot. */
    if (found){
      for ( j = 0; j < mtx->columns; j++){
	mtx->data[i][j] /= pivot;
      }
    }

  }

  return mtx;

}

vm_matrix *vm_rref(vm_matrix *mtx){

  /* First get the row echelon form, then we will reduce it.       */
  if (!vm_row_echelon_form(mtx)) return NULL;

  /* Now begin reducing the matrix to its solutions.               */
  int row = mtx->rows - 1;

  /* Iterate over each row, reducing it based on the previous rows */
  for ( ; row > 0; row--){

    /* Find the pivot for this row. */
    int row_offset = -1;
    int i = 0;
    for ( ; i < mtx->columns; i++){
      if (mtx->data[row][i] != 0){
	row_offset = i;
	break;
      }
    }
    
    if (row_offset == -1) continue;
    /* Otherwise row_offset is the location of the pivot.          */
    
    /* Now, for all the rows above us, we eliminate the value.     */
    int cur_row = 0; // Start at the top of the matrix.
    vm_matrix_row elim_row;
    for ( ; cur_row < row; cur_row++){
      double c = mtx->data[cur_row][row_offset] / mtx->data[row][row_offset];
      
      vm_get_matrix_row(mtx, row, &elim_row);
      _vm_scale_mtx_row(&elim_row, -1 * c);
      _vm_add_rc_to_mtx(mtx, &elim_row, cur_row);
      free(elim_row.data);
    }

  }

  return mtx;

}
